/*
对SimpleFOC进行初始化设置，与官方例程相同，没什么好说的
设置SimpleFOC为力矩控制模式，输入电压12v，限制输出电压10v
注意第64 65行代码，其中电角度需要根据你所搭建的小车的实际情况决定，首次使用时请删除括号内的内容
*/

#include <Arduino.h>
#include <SimpleFOC.h>
#include "FOC_Setting.h"

MagneticSensorI2C  sensor = MagneticSensorI2C(AS5600_I2C);
MagneticSensorI2C  sensor1 = MagneticSensorI2C(AS5600_I2C);
TwoWire  I2Cone = TwoWire(0);
TwoWire  I2Ctwo = TwoWire(1);

// Motor instance
BLDCMotor  motor = BLDCMotor(7);
BLDCDriver3PWM  driver = BLDCDriver3PWM(32, 33, 25, 22);

BLDCMotor  motor1 = BLDCMotor(7);
BLDCDriver3PWM  driver1 = BLDCDriver3PWM(26, 27, 14, 12);

void FOC_Setting()
{
  I2Cone.begin(18, 5, 400000); 
  I2Ctwo.begin(19, 23, 400000);
  sensor.init(&I2Cone);
  sensor1.init(&I2Ctwo);
  // link the motor to the sensor
  motor.linkSensor(&sensor);
  motor1.linkSensor(&sensor1);

  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 12;
  driver.init();

  driver1.voltage_power_supply = 12;
  driver1.init();
  // link the motor and the driver
  motor.linkDriver(&driver);
  motor1.linkDriver(&driver1);
  
  // set motion control loop to be used
  motor.controller = MotionControlType::torque;
  motor1.controller = MotionControlType::torque;

  // contoller configuration 
  // default parameters in defaults.h


  // maximal voltage to be set to the motor
  motor.voltage_limit = 10;
  motor1.voltage_limit = 10;
  // use monitoring with serial 

  // comment out if not needed
  motor.useMonitoring(Serial);
  motor1.useMonitoring(Serial);
  
  //初始化电机
  motor.init();
  motor1.init();
  motor.initFOC(4.20,Direction::CCW); 
  motor1.initFOC(5.20,Direction::CW);
  // ⬆⬆⬆ 首次使用时请删除上面语句括号内的内容,连接串口打开串口监视器
        //SFOC会先正反转进行系统辨识，然后你就把识别出来的内容填进括号里就好了
  Serial.println("Motor ready.");
}

